#ifndef TYPE_TRANSFORM_H
#define TYPE_TRANSFORM_H

#include "quad_msgs/Trajectory.h"
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

namespace Planner
{
    using Eigen::MatrixXd;
    using Eigen::Quaterniond;
    using Eigen::Vector3d;
    using std::pow;
    using std::vector;

    quad_msgs::State getStateFromCoeffs(const vector<double> &coeffsx, const vector<double> &coeffsy,
                                        const vector<double> &coeffsz, double t)
    {
        quad_msgs::State s;
        int order = coeffsx.size();
        for (int i = 0; i < order; i++)
        {
            s.position.x += coeffsx[i] * pow(t, i);
            s.position.y += coeffsy[i] * pow(t, i);
            s.position.z += coeffsz[i] * pow(t, i);
            s.velocity.x += i * coeffsx[i] * pow(t, i - 1);
            s.velocity.y += i * coeffsy[i] * pow(t, i - 1);
            s.velocity.z += i * coeffsz[i] * pow(t, i - 1);
            s.acceleration.x += i * (i - 1) * coeffsx[i] * pow(t, i - 2);
            s.acceleration.y += i * (i - 1) * coeffsy[i] * pow(t, i - 2);
            s.acceleration.z += i * (i - 1) * coeffsz[i] * pow(t, i - 2);
        }
        return s;
    }

    Vector3d getPosFromPieceCoeffs(const vector<double> &coeffsx, const vector<double> &coeffsy,
                                   const vector<double> &coeffsz, double t)
    {
        Vector3d pt;
        pt.setZero();
        int order = coeffsx.size();
        for (int i = 0; i < order; i++)
        {
            pt[0] += coeffsx[i] * pow(t, i);
            pt[1] += coeffsy[i] * pow(t, i);
            pt[2] += coeffsz[i] * pow(t, i);
        }
        return pt;
    }

    void coeffsToTrajMsg(const vector<MatrixXd> &coeffs, const vector<double> &time_seq,
                         vector<quad_msgs::PieceTrajectory> &msgs)
    {
        msgs.clear();
        for (int i = 0; i < time_seq.size(); i++)
        {
            quad_msgs::PieceTrajectory msg;
            msg.duration = time_seq[i];
            for (int j = 0; j < coeffs[0].cols(); j++)
            {
                msg.coeffs_x.push_back(coeffs[0](i, j));
                msg.coeffs_y.push_back(coeffs[1](i, j));
                msg.coeffs_z.push_back(coeffs[2](i, j));
            }
            msgs.push_back(msg);
        }
    }

    void coeffsToGlobalTrajMsg(const vector<MatrixXd> &coeffs, const vector<double> &time_seq,
                               quad_msgs::Trajectory &msg)
    {
        msg.polynomials.clear();
        msg.waypoints.clear();
        msg.order = coeffs[0].cols();
        msg.segments = time_seq.size();

        quad_msgs::State start_state;
        start_state.position.x = coeffs[0](0, 0);
        start_state.position.y = coeffs[1](0, 0);
        start_state.position.z = coeffs[2](0, 0);
        start_state.velocity.x = coeffs[0](0, 1);
        start_state.velocity.y = coeffs[1](0, 1);
        start_state.velocity.z = coeffs[2](0, 1);
        start_state.acceleration.x = 2 * coeffs[0](0, 2);
        start_state.acceleration.y = 2 * coeffs[1](0, 2);
        start_state.acceleration.z = 2 * coeffs[2](0, 2);
        msg.waypoints.push_back(start_state);

        for (int i = 0; i < time_seq.size(); i++)
        {
            quad_msgs::State s;
            quad_msgs::PieceTrajectory pieceTraj;
            pieceTraj.duration = time_seq[i];
            for (int j = 0; j < coeffs[0].cols(); j++)
            {
                pieceTraj.coeffs_x.push_back(coeffs[0](i, j));
                pieceTraj.coeffs_y.push_back(coeffs[1](i, j));
                pieceTraj.coeffs_z.push_back(coeffs[2](i, j));

                s.position.x += coeffs[0](i, j) * pow(time_seq[i], j);
                s.position.y += coeffs[1](i, j) * pow(time_seq[i], j);
                s.position.z += coeffs[2](i, j) * pow(time_seq[i], j);
                s.velocity.x += j * coeffs[0](i, j) * pow(time_seq[i], j - 1);
                s.velocity.y += j * coeffs[1](i, j) * pow(time_seq[i], j - 1);
                s.velocity.z += j * coeffs[2](i, j) * pow(time_seq[i], j - 1);
                s.acceleration.x += j * (j - 1) * coeffs[0](i, j) * pow(time_seq[i], j - 2);
                s.acceleration.y += j * (j - 1) * coeffs[1](i, j) * pow(time_seq[i], j - 2);
                s.acceleration.z += j * (j - 1) * coeffs[2](i, j) * pow(time_seq[i], j - 2);
            }
            msg.waypoints.push_back(s);
            msg.polynomials.push_back(pieceTraj);
        }
    }

    void geometry2EigenVector(const geometry_msgs::Vector3 &msg, Vector3d &pos)
    {
        pos[0] = msg.x;
        pos[1] = msg.y;
        pos[2] = msg.z;
    }

    void geometry2EigenVector(const geometry_msgs::Point &msg, Vector3d &pos)
    {
        pos[0] = msg.x;
        pos[1] = msg.y;
        pos[2] = msg.z;
    }

    void geometry2EigenQuaternion(const geometry_msgs::Quaternion &msg, Quaterniond &orien)
    {
        orien.x() = msg.x;
        orien.y() = msg.y;
        orien.z() = msg.z;
        orien.w() = msg.w;
    }

    template<typename Scalar_t>
    void limit_range(Scalar_t &value, const Scalar_t &limit)
    {
        if (value < -limit)
            value = -limit;
        else if (value > limit)
            value = limit;
    }

    template<typename Scalar_t>
    void limit_range(Scalar_t &value, const Scalar_t &low, const Scalar_t &high)
    {
        if (value < low)
            value = low;
        else if (value > high)
            value = high;
    }
} // namespace Planner

#endif